
/*
* core zero, proc message.
*/

#include <stdint.h>
#include <stdio.h>

#include "rp2040.h"
#include "hardware/clocks.h"
#include "hardware/uart.h"

#include "pico.h"
#include "pico/multicore.h"

#include "umsg.h"
#include "utchan.h"


static inline uint32_t  read_reg( uintptr_t addr )
{
    return (*((volatile uint32_t *) (addr)));
}


static inline void  write_reg( uintptr_t addr, uint32_t value )
{
  *(volatile uint32_t *)(addr) = value;
  return;
}


/*
输入:
u32 : addr
 u8 : cnt
*/
int core_zero_rreg( umsg_t * pmsg )
{
	uint32_t ofs;
	int cnt;
	uint8_t * ptr;
	int i;

	/**/
	ptr = pmsg->data;
	ofs = *(uint32_t *)&ptr[0];
	cnt = (int)ptr[4];
	if ( cnt > 16 ) {
		/* error */
		return 0;
	}

	/**/
	ptr = (uint8_t *)pmsg->data;
	for ( i=0; i<cnt; i++ ) {
		*(uint32_t *)ptr = read_reg( ofs + (i * 4) );
		ptr += 4;
	}

	/**/
	pmsg->mlen = cnt * 4;
	umsg_send( pmsg );
	return 0;
}


int core_zero_wreg( umsg_t * pmsg )
{
	uint32_t ofs;
	uint32_t vvv;
	uint8_t * ptr;
	int cnt;
	int i;

	if ( pmsg->mlen < 4 ) {
		/* error */
		return 1;
	}

	/* addr, v0, v1, ... */
	ptr = (uint8_t *)&pmsg->data[0];
	ofs = *(uint32_t *)ptr;

	ptr += 4;
	cnt = (pmsg->mlen - 4) / 4;

	for ( i=0; i<cnt; i++ ) {
		vvv = *(uint32_t *)ptr;
		write_reg( ofs + (i * 4), vvv );
		ptr += 4;
	}

	/**/
	pmsg->mlen = 4;
	umsg_send( pmsg );
	return 0;	
}


/*
 * 使用字节宽度, 访问总线.
 * 获得一块内存访问的内容.
 * 主要是对 SRAM 有意义.
 * input layout
 * {
 *     uint32_t  addr;
 *      uint8_t  cnt;
 * }
 *
 */
int core_zero_dump( umsg_t * pmsg )
{
	int cnt;
	uint8_t * ptr;
	volatile uint8_t * pmem;
	int i;

	/**/
	ptr = pmsg->data;
	pmem = (volatile uint8_t *)&ptr[0];
	cnt = (int)ptr[4];
	if ( cnt > 63 ) {
		/* error */
		return 0;
	}

	/**/
	ptr = (uint8_t *)pmsg->data;
	for ( i=0; i<cnt; i++ ) {
		ptr[i] = pmem[i];
	}

	/**/
	pmsg->mlen = cnt;
	umsg_send( pmsg );
	return 0;
}


int core_zero_clock( umsg_t * pmsg )
{
	int idx;
	int i;
	uint8_t * ptr;
	uint32_t  hzhz;

	/**/
	idx = pmsg->data[0];
	hzhz = clock_get_hz( idx );

	/**/
	ptr = (uint8_t *)&pmsg->data[0];
	*(uint32_t *)ptr = hzhz;

	/**/
	pmsg->mlen = 4;
	umsg_send( pmsg );
	return 0;
}



void  core_zero_proc_msg( void )
{
	umsg_t * pmsg;
	int mlen;


	while ( true )  {

		pmsg = umsg_recv();
		if ( pmsg == NULL ) {
			break;
		}

		/* check length */
		if ( pmsg->mlen <= 0 ) {
			umsg_free( pmsg );
			continue;
		}

		/**/
		mlen = pmsg->mlen - 1;
		pmsg->type = pmsg->data[mlen];
		pmsg->mlen = (uint8_t)mlen;


		if ( pmsg->type < 0x80 ) {
			/* type = [0x40,0x7F] :  forward to dut */
			utchan_send( pmsg );
			continue;
		}

		// printf( "recv, %p\n", pmsg );

		switch ( pmsg->type) {

		case 0x80:
			core_zero_rreg( pmsg );
			break;

		case 0x81:
			core_zero_wreg( pmsg );
			break;

		case 0x82:
			core_zero_dump( pmsg );
			break;

		case 0x90:
			core_zero_clock( pmsg );
			break;

		default:
			/* type = [0x00,0x3F] : forward to core-1 */
			pmsg->type = pmsg->type & 0x3F;
			multicore_fifo_push_blocking( (uint32_t)(uintptr_t)pmsg );
			break;

		}

	}

	return;
}


